﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Windows.Forms;
using QuadRemote.Core;

namespace QuadRemote.Panels
{

    class BarometerPanel : Graph
    {
        private Queue<float> qx;
        private Queue<float> qxf;
        private long lastUpdate;
        private int waitSamples = 1;

        private GraphData x;
        private GraphData xf;

        public BarometerPanel()
            : base("Barometer", 30, 5, Color.Magenta)
        {

            qx = new Queue<float>();
            qxf = new Queue<float>();

            _gain = 0.995F;

            x = new GraphData();
            x.display = GraphDisplay.REALTIME;
            x.name = "Raw";
            x.scale = 1;
            x.type = GraphType.NORMAL;
            x.unit = GraphUnit.FLOAT;
            x.pen = new Pen(Color.FromArgb(255, 0, 0), 2);

            xf = new GraphData();
            xf.display = GraphDisplay.REALTIME;
            xf.name = "Filtered";
            xf.scale = 1;
            xf.type = GraphType.NORMAL;
            xf.unit = GraphUnit.FLOAT;
            xf.pen = new Pen(Color.FromArgb(128, 0, 0), 2);

            graphs.Add(x);

            graphs.Add(xf);

            init();
        }
        private float vv = 0;
        public void queue(float val, float temp)
        {
            float gain = 0.95f;
            vv = vv * gain + val * (1.0f - gain);
            qx.Enqueue(vv);
            qxf.Enqueue(temp);
            
        }

        private float dynamicF = 20;

        public override void PaintPanel(System.Drawing.Graphics g)
        {
            int[] sizes = { qx.Count, qxf.Count};

            int max = sizes[0];
            if (sizes[1] > max) max = sizes[1];

            Queue<float>[] queues = { qx, qxf};
            GraphData[] graphs = { x, xf};

            if (max >= waitSamples)
            {
                float val;

                for (int o = 0; o < sizes.Length; o++)
                {
                    int bufferBegin = bufferOffset;

                    if (sizes[o] == 0)
                    {
                        val = graphs[o].data[bufferBegin];

                        for (int i = 0; i < max; i++)
                        {
                            bufferBegin++;
                            if (bufferBegin == bufferSize) bufferBegin = 0;
                            graphs[o].data[bufferBegin] = val;
                        }
                        continue;
                    }

                    float t = 0;

                    lock (queues[o])
                    {
                        val = queues[o].Dequeue();

                        for (int i = 0; i < max; i++)
                        {
                            bufferBegin++;
                            if (bufferBegin == bufferSize) bufferBegin = 0;
                            graphs[o].data[bufferBegin] = val;

                            t += max / sizes[o];
                            if (t >= 1 && queues[o].Count > 0)
                            {
                                val = queues[o].Dequeue();
                                t -= 1;
                            }
                        }
                    }
                }

                bufferOffset += max;
                if (bufferOffset >= bufferSize) bufferOffset -= bufferSize;


                long tickDiff = ((DateTime.Now.Ticks - lastUpdate) / TimeSpan.TicksPerMillisecond);
                if (tickDiff != 0)
                {
                    float f = 1000.0F / tickDiff * waitSamples;

                    dynamicF = (int)(dynamicF * 0.98F + f * 0.02F);
                    _freq = (int)Math.Round(dynamicF);
                }

                lastUpdate = DateTime.Now.Ticks;
            }

            base.PaintPanel(g);
        }
    }
}
